/**
  ******************************************************************************
  * @file           : bsp_control.c
  * @author         : Ylint
  * @brief          : None
  * @attention      : None
  * @date           : 2024/4/9
  ******************************************************************************
  */

#include "bsp_control.h"
#include "bsp_led.h"
#include "bsp_pid_assistant.h"
#include "tim.h"
#include "bsp_pid.h"
#include "bsp_mpu6050.h"
#include "stdlib.h"
#include "kalman.h"



#define _ANOMALYDETECTION_COUNT     5


int Vertical_Out;   //直立环PID计算结果。
int Velocity_Out;   //速度环PID计算结果。
int Turn_Out;     //转向环PID计算结果。
int MOTO1, MOTO2;   //电机PWM波输出。
int Encoder_Left, Encoder_Right;    //编码器数值

uint8_t CAR_EXCEPTIONAL_SITUATION = 0;    //小车异常标志



/**
  * @brief  获取定时器捕获到的编码器数值。
  * @param  htim：TIM定时器。
  * @retval temp：编码器数值。
  */
int GET_Encoder(TIM_HandleTypeDef *htim)
{
  int temp;

  //获取编码器数值。
  temp = (short) __HAL_TIM_GET_COUNTER(htim);
          __HAL_TIM_SetCounter(htim, 0);

  return temp;
}

/**
  * @brief  PID计算结果PWM限幅。
  * @param  MotoA：电机1PWM占空比数值的地址.
  * @param  MotoB：电机2PWM占空比数值的地址。
  * @retval None
  */
static void PWM_Limit(int *MotoA, int *MotoB) {
  if (*MotoA > _PWM_OUT_MAX) *MotoA = _PWM_OUT_MAX;
  if (*MotoA < _PWM_OUT_MIN) *MotoA = _PWM_OUT_MIN;
  if (*MotoB > _PWM_OUT_MAX) *MotoB = _PWM_OUT_MAX;
  if (*MotoB < _PWM_OUT_MIN) *MotoB = _PWM_OUT_MIN;
}

/**
  * @brief  PWM输出
  * @param  motoA：电机1PWM波的占空比。
  * @param  motoB：电机2PWM波的占空比。
  * @retval None
  */
void Motor_PWM_Out(int motoA, int motoB) {
  if (motoA < 0) {
    HAL_GPIO_WritePin(M3IN1_GPIO_Port, M3IN1_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(M3IN2_GPIO_Port, M3IN2_Pin, GPIO_PIN_RESET);
  } else {
    HAL_GPIO_WritePin(M3IN1_GPIO_Port, M3IN1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(M3IN2_GPIO_Port, M3IN2_Pin, GPIO_PIN_SET);
  }
  if (motoB < 0) {
    HAL_GPIO_WritePin(M4IN1_GPIO_Port, M4IN1_Pin, GPIO_PIN_RESET);
    HAL_GPIO_WritePin(M4IN2_GPIO_Port, M4IN2_Pin, GPIO_PIN_SET);
  } else {
    HAL_GPIO_WritePin(M4IN1_GPIO_Port, M4IN1_Pin, GPIO_PIN_SET);
    HAL_GPIO_WritePin(M4IN2_GPIO_Port, M4IN2_Pin, GPIO_PIN_RESET);
  }
          __HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_3, abs(motoA));
          __HAL_TIM_SetCompare(&htim8, TIM_CHANNEL_4, abs(motoB));
}



/**
  * @brief  小车异常检测。
  * @param  out：PWM输出占空比。
  * @retval ‘1’：异常；‘0’：无异常。
  */
uint8_t CAR_AnomalyDetection(int out)
{
  static uint8_t count = 0;

  if(Roll >=40 || Roll <= -40)
  {
    count++;
    if(count >= _ANOMALYDETECTION_COUNT)
    {
      count = 0;
      return 1;   //确认小车已经倒地。
    }
    return 0;
  } else if ((Roll <= 20 && Roll >= -20) && (abs(Encoder_Left) >= 75) && (abs(Encoder_Right) >= 75))
  {
    count++;
    if (count >= _ANOMALYDETECTION_COUNT)
    {
      count = 0;
      return 1;   //确认小车被拿起。
    }
    return 0;
  } else if ((abs(PID_Struct_Velocity.PID_Actual_Val) <= 4) && (abs(out) >= 2000))
  {
    count++;
    if (count >= _ANOMALYDETECTION_COUNT)
    {
      count = 0;
      return 1;   //确认小车被挡住。
    }
    return 0;
  } else
  {
    count = 0;    //小车无异常。
  }

  return 0;
}

/**
  * @brief  小车着陆检测。
  * @param  None
  * @retval None
  */
void CAR_LandingDetection(void)
{
  static uint8_t count = 0;

  count++;
  if (count >= _ANOMALYDETECTION_COUNT)
  {
    count = 0;
    if (Roll <=5 && Roll >= -5)
    {
      CAR_EXCEPTIONAL_SITUATION = 0;
    }
  }
}

/**
  * @brief  平衡小车控制函数
  * @param  None
  * @retval None
  */
void Motor_Control(void) {
  int PWM_Out;


  //1.读取编码器数据和陀螺仪数据。
  Encoder_Left = GET_Encoder(&htim4);
  Encoder_Right = -GET_Encoder(&htim5);
  MPU6050_DMP_Get_Data(&Pitch, &Roll, &Yaw);
  //理论上可以不用对Roll值进行卡尔曼滤波。
//  kalman_update(&kf, Roll, 0.01);
//  Roll = kf.x_hat;



  //2.将数据传入PID控制器。
  PID_Struct_Velocity.PID_Actual_Val = Encoder_Left + Encoder_Right;
  //(1)计算速度环PID。
  Velocity_Out = PID_Velocity();
  //(2)串级PID将速度环输出结果传递给直立环。
  PID_Struct_Vertical.PID_Actual_Val = Roll - Velocity_Out;
  Vertical_Out = PID_Vertical(Gyro[0]);
  //(3)计算PID转向环。
  Turn_Out = PID_Turn(Gyro[2]);

  //3.将PID控制器调节值输出给电机。
  //(1)得到PWM输出。
  PWM_Out = Vertical_Out;
  //(2)融合转向环。
  MOTO1 = PWM_Out + Turn_Out;
  MOTO2 = PWM_Out - Turn_Out;
  //(3)输出限幅。
  PWM_Limit(&MOTO1, &MOTO2);
  //(4)电机输出。（输出可以考虑滤波处理）
  //1.着陆检测。
  if (CAR_EXCEPTIONAL_SITUATION == 1)    //小车还未着陆。
  {
    CAR_LandingDetection();
  }
  //2.小车异常检测。
  if (CAR_EXCEPTIONAL_SITUATION == 0)
  {
    CAR_EXCEPTIONAL_SITUATION = CAR_AnomalyDetection(PWM_Out);
  }
  //3.小车输出
  if (CAR_EXCEPTIONAL_SITUATION == 1)    //检测到小车异常，电机无输出。
  {
    Motor_PWM_Out(0, 0);
    PID_Struct_Init(&PID_Struct_Vertical, CURVES_CH1);
    PID_Struct_Init(&PID_Struct_Velocity, CURVES_CH2);
    PID_Struct_Init(&PID_Struct_Turn, CURVES_CH3);
    //1.PID-PD直立环。
    SET_PID_Param(&PID_Struct_Vertical,888,0,2.103);
    //2.PID-PI速度环。
    SET_PID_Param(&PID_Struct_Velocity,-0.1666,-0.0022,0);
    PID_Struct_Velocity.LPF = 0.7;
    //3.PID-PD转向环。
    SET_PID_Param(&PID_Struct_Turn,1.403,0,0);
    _LED_BLUE_Toggle;
  } else
  {
    Motor_PWM_Out(MOTO1, MOTO2);    //小车无异常，电机正常输出。
  }

}





